#!/usr/bin/env python

import message_filters
import rospy
from geometry_msgs.msg import PoseStamped
import csv

PATH = "/home/parallels/ws/src/bag2csv/data/"

def callback(uav1, uav2, uav3, uav4, v_uav1, v_uav2, v_uav3, v_uav4):
    with open(PATH + 'uav.csv', 'a') as csvfile:
        uav1_writer = csv.writer(csvfile)
        uav1_writer.writerow([uav1.header.stamp, uav1.pose.position.x, uav1.pose.position.y, uav1.pose.position.z,
                              uav2.header.stamp, uav2.pose.position.x, uav2.pose.position.y, uav2.pose.position.z,
                              uav3.header.stamp, uav3.pose.position.x, uav3.pose.position.y, uav3.pose.position.z,
                              uav4.header.stamp, uav4.pose.position.x, uav4.pose.position.y, uav4.pose.position.z,
                              v_uav1.header.stamp, v_uav1.pose.position.x, v_uav1.pose.position.y, v_uav1.pose.position.z,
                              v_uav2.header.stamp, v_uav2.pose.position.x, v_uav2.pose.position.y, v_uav2.pose.position.z,
                              v_uav3.header.stamp, v_uav3.pose.position.x, v_uav3.pose.position.y, v_uav3.pose.position.z,
                              v_uav4.header.stamp, v_uav4.pose.position.x, v_uav4.pose.position.y, v_uav4.pose.position.z])




with open(PATH+'uav.csv', 'a') as csvfile:
    uav1_writer = csv.writer(csvfile, lineterminator='\n')
    uav1_writer.writerow(['uav1_stamp', 'uav1_x', 'uav1_y', 'uav1_z',
                          'uav2_stamp', 'uav2_x', 'uav2_y', 'uav2_z',
                          'uav3_stamp', 'uav3_x', 'uav3_y', 'uav3_z',
                          'uav4_stamp', 'uav4_x', 'uav4_y', 'uav4_z',
                          'vicon_uav1_stamp', 'vicon_uav1_x', 'vicon_uav1_y', 'vicon_uav1_z',
                          'vicon_uav2_stamp', 'vicon_uav2_x', 'vicon_uav2_y', 'vicon_uav2_z',
                          'vicon_uav3_stamp', 'vicon_uav3_x', 'vicon_uav3_y', 'vicon_uav3_z',
                          'vicon_uav4_stamp', 'vicon_uav4_x', 'vicon_uav4_y', 'vicon_uav4_z'])


rospy.init_node('reader', anonymous=True)
uav1_sub = message_filters.Subscriber("/uav1/mavros/local_position/pose", PoseStamped)
uav2_sub = message_filters.Subscriber("/uav2/mavros/local_position/pose", PoseStamped)
uav3_sub = message_filters.Subscriber("/uav3/mavros/local_position/pose", PoseStamped)
uav4_sub = message_filters.Subscriber("/uav4/mavros/local_position/pose", PoseStamped)

v_uav1_sub = message_filters.Subscriber("/UAV1/viconros/mocap/pos", PoseStamped)
v_uav2_sub = message_filters.Subscriber("/UAV2/viconros/mocap/pos", PoseStamped)
v_uav3_sub = message_filters.Subscriber("/UAV3/viconros/mocap/pos", PoseStamped)
v_uav4_sub = message_filters.Subscriber("/UAV4/viconros/mocap/pos", PoseStamped)

ts = message_filters.ApproximateTimeSynchronizer([uav1_sub, uav2_sub, uav3_sub,uav4_sub,
                                                  v_uav1_sub, v_uav2_sub, v_uav3_sub, v_uav4_sub], 50, 0.1)
ts.registerCallback(callback)

rospy.spin()